#include "stdio.h"
#include "stdint.h"
#include "DevicesPID.h"


#define PID_ABS(x) (((x) > 0.0f) ? (x) : (0.0f - (x)))


void vPIDDeInit(PIDInfoType *ptypeHandle, float fkp, float fki, float fkd, float fErrorMax, float fIntegralMax, float fIntegralMin, float fExportValueMax, float fExportValueMin)
{
    ptypeHandle->exportValue    = 0.0f;
    ptypeHandle->errLast        = 0.0f;
    ptypeHandle->integral       = 0.0f;
    ptypeHandle->kp             = fkp;
    ptypeHandle->ki             = fki;
    ptypeHandle->kd             = fkd;

    ptypeHandle->errMax         = fErrorMax;
    ptypeHandle->integralMax    = fIntegralMax;
    ptypeHandle->integralMin    = fIntegralMin;
    ptypeHandle->exportValueMax = fExportValueMax;
    ptypeHandle->exportValueMin = fExportValueMin;
}

/*
Return Value:   PID调节后的输出值
Parameters:     fSet: 设定值;
                fNow: 当前值;
Description:    位置型PID调节算法
*/
float fPIDLocation(PIDInfoType *ptypePID, float fSet, float fNow)
{
    float fError = fSet - fNow;

    /* 防止积分超调 */
    if(PID_ABS(fError) > ptypePID->errMax)
        ptypePID->integral = 0.0f;

    ptypePID->integral += ptypePID->ki * fError;

    /* 限定积分上、下限 */
    ptypePID->integral = (ptypePID->integral > ptypePID->integralMax) ? ptypePID->integralMax : ptypePID->integral;
    ptypePID->integral = (ptypePID->integral < ptypePID->integralMin) ? ptypePID->integralMin : ptypePID->integral;

    /* PID 运算 */
    ptypePID->exportValue = ptypePID->kp * fError
                          + ptypePID->integral
                          + ptypePID->kd * (fError - ptypePID->errLast);

    ptypePID->errLast = fError;

    /* 限定输出上、下限 */
    ptypePID->exportValue = (ptypePID->integral > ptypePID->exportValueMax) ? ptypePID->exportValueMax : ptypePID->exportValue;
    ptypePID->exportValue = (ptypePID->integral < ptypePID->exportValueMin) ? ptypePID->exportValueMin : ptypePID->exportValue;

    return ptypePID->exportValue;
}
